package com.gitee.wsl.mathematics.complex.quaternion.ext

import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.mathematics.vector.vec4.Vector4

//internal fun xform(v: Vector3): Vector3 {
//    if (GodotJvmBuildConfig.DEBUG) {
//        require(isNormalized()) {
//            "The quaternion must be normalized."
//        }
//    }
//
//    val u = Vector3(x, y, z)
//    val uv = u.cross(v)
//
//    return v + (uv * w + u.cross(uv)) * 2.0f
//}

operator fun<T:Number,V: Vector3<T, V>> Quaternion<T, *>.times(point:Vector3<T, V>): V {
    val x = x * 2.0
    val y = y * 2.0
    val z = z * 2.0
    val xx = this.x * x
    val yy = this.y * y
    val zz = this.z * z
    val xy = this.x * y
    val xz = this.x * z
    val yz = this.y * z
    val wx = w * x
    val wy = w * y
    val wz = w * z
    return point.create(
        (one - (yy + zz)) * point.x + (xy - wz) * point.y + (xz + wy),
        (xy + wz) * point.x + (one - (xx + zz)) * point.y + (yz - wx),
        (xz - wy) * point.x + (yz + wx) * point.y + (one - (xx + yy))
    ) * point.z
}

operator fun<T:Number,V: Quaternion<T, V>> Quaternion<T, V>.times(b: Vector4<T,*>):V {
    val resW = w * b.w - x * b.x - y * b.y - z * b.z
    val resX = w * b.x + x * b.w + y * b.z - z * b.y
    val resY = w * b.y + y * b.w + z * b.x - x * b.z
    val resZ = w * b.z + z * b.w + x * b.y - y * b.x

    return create(resW, resX, resY, resZ)
}
